
local AStar = {}

--[[
param s start point
param e end point
param g grid map
param w grid width
param h grid height
]]

function AStar.newFindPath(s, e, g, w, h) --只是删除一些旧的path中得point 这个路径相邻元素间距离不一样 要算一下获得时间
	local path = AStar.findPath(s, e, g, w, h)
	if not path then return end
	local newPath = {path[1],}
	for i = 1, #path do
		if not path[i + 2] then
			break
		end 
		--判断是否需要把 path[i + 1] 加入newPath
		local crossedPoints = _getCrossedPoints(newPath[#newPath], path[i+2])
		local isHasBarrier = false
		for _, v in pairs(crossedPoints) do
			if not g[v.y][v.x] then
				isHasBarrier = true
				break
			end	
		end

		if isHasBarrier then 
			table.insert(newPath, path[i+1])
		end

	end
	table.insert(newPath, path[#path])
	return newPath  
end

function _getCrossedPoints(fromPos, toPos)
	local points = {}
	--以fromPos为原点 建立坐标系
	local k = (toPos.y - fromPos.y)/(toPos.x - fromPos.x)

	--遍历所有两点间的方格
	for i = 0 , math.abs(toPos.x - fromPos.x) do
		for j = 0 , math.abs(toPos.y - fromPos.y) do
			if (i == 0 and j == 0) or (i == toPos.x - fromPos.x and j == toPos.y - fromPos.y) then
			else
				--k * x - y = 0--线段方程
				local x, y 
				if toPos.x - fromPos.x < 0 then
					x = -i
				else
					x = i
				end
				if toPos.y - fromPos.y < 0 then
					y = -j
				else
					y = j
				end
				--点到直线距离
				local dis = (k*x - y)/ math.sqrt(math.pow(k,2) + 1)
				dis = math.abs(dis)
				if dis < 0.5 * math.sqrt(2) then
					table.insert(points,{x = fromPos.x + x,y = fromPos.y + y})
				end
			end
		end	
	end

	return points
end

function AStar.findPath(s, e, g, w, h)
	local startPoint = clone(s)
	local endPoint = clone(e)
	local width = w
	local height = h
	local grid = g
	local cur
	local EndPoint
	local isFindPath

	-- dump(startPoint, "startPoint:")
	-- dump(endPoint, "endPoint:")
	-- printInfo("width:%d, height:%d", width, height)

	-- local temp = {x = 312, y = 1}
	-- printInfo("312,1  %s", tostring(grid[temp.x][temp.y]))

	AStar.init()

	local open = AStar.open_
	local close = AStar.close_

	-- add start to open

	open[startPoint.x * 1000 + startPoint.y] = startPoint

	while true do

		if nil == next(open) then
			-- havn't find path
			break
		end

		-- find min f value point
		cur = nil
		for _, v in pairs(open) do
			if nil == cur then
				cur = v
			elseif cur.f > v.f then
				cur = v
			end
		end

		if cur.x * 1000 + cur.y == endPoint.x * 1000 + endPoint.y then
			EndPoint = cur
			-- dump(cur, "find end point:")
			-- find path
			isFindPath = true
			break
		end

		-- add the close
		close[cur.x * 1000 + cur.y] = cur
		open[cur.x * 1000 + cur.y] = nil

		-- add the neighbouring to open
		AStar.addPointIf({x = cur.x, y = cur.y - 1}, cur, endPoint, grid, width, height) -- up
		AStar.addPointIf({x = cur.x, y = cur.y + 1}, cur, endPoint, grid, width, height) -- down
		AStar.addPointIf({x = cur.x - 1, y = cur.y}, cur, endPoint, grid, width, height) -- left
		AStar.addPointIf({x = cur.x + 1, y = cur.y}, cur, endPoint, grid, width, height) -- right


		AStar.addPointIf({x = cur.x+1, y = cur.y - 1}, cur, endPoint, grid, width, height,true) -- up
		AStar.addPointIf({x = cur.x-1, y = cur.y + 1}, cur, endPoint, grid, width, height,true) -- down
		AStar.addPointIf({x = cur.x - 1, y = cur.y-1}, cur, endPoint, grid, width, height,true) -- left
		AStar.addPointIf({x = cur.x + 1, y = cur.y+1}, cur, endPoint, grid, width, height,true) -- right
	end

	local path
	if isFindPath then
		path = {}
		cur = EndPoint

		while cur do
			table.insert(path, 1, {x = cur.x, y = cur.y})
			cur = cur.parent
		end
	end

	-- dump(path, "astar find path:")

	return path
end

function AStar.init()
	AStar.open_ = {}
	AStar.close_ = {}
end

function AStar.addPointIf(p, parent, endPoint, grid, width, height,isInclined)
	if p.x < 1 or p.x > width
		or p.y < 1 or p.y > height then
		-- invalid point
		return
	end

	-- printInfo("grid (%d,%d) %s", p.x, p.y, tostring(grid[p.y][p.x]))
	-- dump(grid[p.y], "line:")
	if not grid[p.y][p.x] then
		return
	end

	if AStar.close_[p.x * 1000 + p.y] then
		return
	end

	AStar.calcPointInfo(p, parent, endPoint,isInclined)

	local pKey = p.x * 1000 + p.y
	if not AStar.open_[pKey] or AStar.open_[pKey].f > p.f then
		AStar.open_[pKey] = p
	end
end

function AStar.calcPointInfo(p, parent, endPoint,isInclined)
	
	if isInclined  then
		p.g = (parent.g or 0) + 14
	else
		p.g = (parent.g or 0) + 10	
	end
	
	p.h = (math.abs(endPoint.x - p.x) + math.abs(endPoint.y - p.y)) * 10
	p.f = p.g + p.h
	p.parentKey = parent.x * 1000 + parent.y
	p.parent = parent
end

return AStar
